/***************************************************************************************

***************************************************************************************/



/*
*************************************************************************************
* File name: 
* Function: 
*
*************************************************************************************
*/

#include <stdlib.h>
#include <math.h>
#include <memory.h>
#include <assert.h>
#include "rdopt_coding_state.h"
#include "memalloc.h"
#include "refbuf.h"
#include "block.h"
#include "vlc.h"
#include "macroblock.h"

#include "global.h"
#include "AEC.h"         

#ifdef FastME
#include "fast_me.h"
#endif
#include "wquant.h"

#ifdef TDRDO
#include "tdrdo.h"
#endif

//==== MODULE PARAMETERS ====
int   best_mode;
int   best_trans;
int   best_splite[4];
int   rec_mbY[16][16], rec_mbU[/*8*/16][8], rec_mbV[/*8*/16][8], rec_mbY8x8[16][16];   
int   mpr8x8[16][16];
int   *cofAC_MD[6][4][2];        // [8x8block][4x4block][level/run][scan_pos]
int   *cofAC_IC[6][4][2];        // [8x8block][4x4block][level/run][scan_pos]
int   cbp, cbp8x8, cnt_nonz_8x8; 
int   frefframe[2][2], brefframe[2][2], b8mode[4], b8pdir[4];
int   best8x8mode [4];                // [block]
int   best8x8pdir [MAXMODE][4];       // [mode][block]
int   best8x8ref  [MAXMODE][4];       // [mode][block]
int   b8_intra_pred_modes[4];
CSptr cs_mb=NULL, cs_b8=NULL, cs_cm=NULL;

#ifdef RDO_Q
CSptr cs_rdoq = NULL;
#endif

int   best_c_imode;

int   best8x8bwref     [MAXMODE][4];       // [mode][block]
int   best8x8symref     [MAXMODE][4][2];       // [mode][block]

int   best_intra_pred_modes_tmp[16];
int   best_mpr_tmp[16][16];

#define IS_FW (((best8x8pdir[mode][k]==0 || best8x8pdir[mode][k]==2 || best8x8pdir[mode][k] == 3) && (mode!=P8x8 || best8x8mode[k]!=0 || !bframe)) || (best8x8pdir[mode][k]==3 && (!bframe)))
#define IS_BW ((best8x8pdir[mode][k]==1 || best8x8pdir[mode][k]==2) && (mode!=P8x8 || best8x8mode[k]!=0))

/*
*************************************************************************
* Function:delete structure for RD-optimized mode decision
* Input:
* Output:
* Return: 
* Attention:
*************************************************************************
*/

void swap_cofAC(int *a[6][4][2], int *b[6][4][2])
{
    int size = 6 * 4 * 2 * sizeof(int*);
    int *t[6][4][2];
    memcpy(t, a, size);
    memcpy(a, b, size);
    memcpy(b, t, size);
}

void clear_rdopt ()
{
    free_mem_ACcoeff(cofAC_MD);
    free_mem_ACcoeff(cofAC_IC);

  // structure for saving the coding state
  delete_coding_state (cs_mb);
  delete_coding_state (cs_b8);
  delete_coding_state (cs_cm);

#ifdef RDO_Q
  delete_coding_state (cs_rdoq);
#endif

}

/*
*************************************************************************
* Function:create structure for RD-optimized mode decision
* Input:
* Output:
* Return: 
* Attention:
*************************************************************************
*/

void init_rdopt ()
{
    get_mem_ACcoeff(cofAC_MD);
    get_mem_ACcoeff(cofAC_IC);

	// structure for saving the coding state
	cs_mb  = create_coding_state ();
	cs_b8  = create_coding_state ();
	cs_cm  = create_coding_state ();

#ifdef RDO_Q
	cs_rdoq = create_coding_state();
#endif

}

/*
*************************************************************************
* Function:Get RD cost for IVC intra block
* Input:
* Output:
* Return: 
* Attention:
*************************************************************************
*/

double RDCost_for_IVCIntraBlocks(int *nonzero,
								 int b8,
								 int b4,
								 int ipmode,
								 double lambda,
								 double  min_rdcost,
								 int bsize)
{
    int x, y, tmp_cbp;
    int block_x;
    int block_y;
    int pic_pix_x;
    int pic_pix_y;
    int even_block;
    int tmp_block_88_inv[MB_BLOCK_SIZE*MB_BLOCK_SIZE];
    Macroblock  *currMB;
    SyntaxElement *currSE;
    int incr_y = 1, off_y = 0;
    Slice *currSlice = img->currentSlice;
    int rate;
    int distortion;
    DataPartition  *dataPart;


    block_x = 8 * (b8 % 2) + 4 * (b4 % 2);
    block_y = 8 * (b8 / 2) + 4 * (b4 / 2);
    pic_pix_x = img->pix_x + block_x;
    pic_pix_y = img->pix_y + block_y;

    even_block = 0;
    currMB = &img->mb_data[img->current_mb_nr];
    currSE = &img->MB_SyntaxElements[currMB->currSEnr];
    //===== perform DCT, Q, IQ, IDCT, Reconstruction =====

    for (y = 0; y < bsize; y++) {
        for (x = 0; x < bsize; x++) {
            tmp_block_88_inv[y*bsize+x] = img->m7[x][y];        //the subblock to be processed is in the top left corner of img->m7[][].
        }
    }

    tmp_cbp = 0;

    transform_B8(tmp_block_88_inv, bsize);
    scanquant_B8(img->qp - MIN_QP, 4, b8, b4, tmp_block_88_inv, 0, &tmp_cbp
#ifdef RDO_Q
        , lambda
#endif
        , bsize); // '|4' indicate intra for quantization		

    *nonzero = (tmp_cbp != 0);

    //===== get distortion (SSD) of 8x8 block =====
    distortion = 0;

    /*lgp*/
    for (y = 0; y < bsize; y++) {
        for (x = 0; x < bsize; x++) {
			distortion += img->quad[imgY_org[(pic_pix_y + incr_y*y + off_y)*(img->width) + pic_pix_x + x] - imgY[(pic_pix_y + incr_y*y + off_y)*(img->iStride) + pic_pix_x + x]];
        }
    }

    //===== RATE for INTRA PREDICTION MODE  (SYMBOL MODE MUST BE SET TO UVLC) =====
    currSE->value1 = ipmode;
    //--- set position and type ---
    currSE->context = 4 * b8;
    currSE->type = SE_INTRAPREDMODE;

    currSE->writing = writeIntraPredMode_AEC;
    //--- choose data partition ---
    dataPart = &(currSlice->partArr[0]);
    dataPart->writeSyntaxElement(currSE, dataPart);

    rate = currSE->len;
    currSE++;
    currMB->currSEnr++;

    //===== RATE for LUMINANCE COEFFICIENTS =====

    x = currMB->cbp;
    currMB->cbp = tmp_cbp;// needs a correct Macroblock->cbp .
    rate += writeLumaCoeff8x8_AEC(b8, b4, 1, bsize);
    currMB->cbp = x;

    //calc RD and return it.
    return (double)distortion + lambda*(double)rate;
}

/*
*************************************************************************
* Function:Mode Decision for IVC intra blocks
* Input:
* Output:
* Return: 
* Attention:
*************************************************************************
*/
int Mode_Decision_for_IVC_IntraBlocks(int b8, int b4, double lambda, int *min_cost, int bsize)
{
    int ipmode, best_ipmode, i, j;
    int c_nz, nonzero;
    double rdcost;
    int block_x;
    int block_y;
    int pic_pix_x;
    int pic_pix_y;
    Macroblock *currMB;
    double min_rdcost;
    const int incr_y = 1;
    const int off_y = 0;
    int MBRowSize = img->width / MB_BLOCK_SIZE;
    int x;
    int y;
    int  best_rec[16][16];
    int  best_coef[2][257];
    int coef_num = bsize * bsize;
    int b4_x, b4_y;
    currMB = img->mb_data + img->current_mb_nr;


    block_x = 8 * (b8 & 1) + 4 * (b4 & 1);
    block_y = 8 * (b8 >> 1) + 4 * (b4 >> 1);
    pic_pix_x = img->pix_x + block_x;
    pic_pix_y = img->pix_y + block_y;
    b4_x = pic_pix_x >> 2;
    b4_y = pic_pix_y >> 2;
    min_rdcost = 1e30;
    *min_cost = (1 << 20);

    //===== INTRA PREDICTION FOR 8x8 BLOCK =====
    intrapred_luma_IVC(pic_pix_x, pic_pix_y, bsize);

    ipmode = 2;
    for (ipmode = 0; ipmode < NO_INTRA_PMODE; ipmode++) {
        if (img->mprr_flag[ipmode][0] >= 0)        //changed this. the intrapred function marks the invalid modes. At least one is always valid (the DC).
        {
            // get prediction and prediction error
			for (j = 0; j < bsize; j++) {
				for (i = 0; i < bsize; i++) {
					img->mpr[block_x + i][block_y + j] = img->mprr[ipmode][j * 16 + i];
					img->m7[i][j] = imgY_org[(pic_pix_y + incr_y*j + off_y)*(img->width) + pic_pix_x + i] - img->mpr[block_x + i][block_y + j];
				}
			}

            store_coding_state(cs_cm);

            // get and check rate-distortion cost
            if ((rdcost = RDCost_for_IVCIntraBlocks(&c_nz, b8, b4, ipmode, lambda, min_rdcost, bsize)) < min_rdcost) {
                //--- set coefficients ---
                for (j = 0; j < 2; j++) {
                    memcpy(best_coef[j], img->cofAC[b8][b4][j], (coef_num + 1) * sizeof(int));
                }

                //--- set reconstruction ---
                for (y = 0; y < bsize; y++) {
                    for (x = 0; x < bsize; x++) {
						best_rec[y][x] = imgY[(pic_pix_y +/*y*/incr_y*y + off_y/*lgp*/)*(img->iStride) + pic_pix_x + x];
                    }
                }

                //--- flag if dct-coefficients must be coded ---
                nonzero = c_nz;

                //--- set best mode update minimum cost ---
                min_rdcost = rdcost;
                *min_cost = (int)min_rdcost;
                best_ipmode = ipmode;
            }
            reset_coding_state(cs_cm);
        }
    }

    //===== set intra mode prediction =====
    currMB->intra_pred_modes[b8 * 4 + b4] = best_ipmode;

    //===== restore coefficients =====
    for (j = 0; j < 2; j++) {
        memcpy(img->cofAC[b8][b4][j], best_coef[j], (coef_num + 1) * sizeof(int));
    }

    //--- set reconstruction ---
    for (y = 0; y < bsize; y++) {
        for (x = 0; x < bsize; x++) {
			imgY[(pic_pix_y + incr_y*y + off_y)*(img->iStride) + pic_pix_x + x] = best_rec[y][x];
        }
    }

    return nonzero;
}


/*
*************************************************************************
* Function:Mode Decision for an 8x8 Intra block
* Input:
* Output:
* Return: 
* Attention:
*************************************************************************
*/

int Mode_Decision_for_8x8IntraBlocks(int b8, double lambda, int *cost)
{
    int cbp_8 = 0, cbp_4 = 0;
    int cost_tmp;
    int cost_8 = 0;
    int cost_4 = 0;
    int b4;
    Macroblock *currMB = img->mb_data + img->current_mb_nr;
    int ipred8[4];
    int coef8[2][65];
    byte rec8[8][8];
    int  mpr8[8][8];
    int stx, sty;
    int x, y;

    // try 8x8
    currMB->trans_split[b8] = 0;
    if (Mode_Decision_for_IVC_IntraBlocks(b8, 0, lambda, &cost_tmp, 8)) {
        cbp_8 = 0xF << (b8 * 4);
    }
    cost_8 = cost_tmp + (int)floor(2.0 * lambda + 0.4999);

    // backup 8x8
    memcpy(ipred8, &currMB->intra_pred_modes[b8*4], sizeof(int) * 4);
    memcpy(coef8[0], img->cofAC[b8][0][0], 65 * sizeof(int));
    memcpy(coef8[1], img->cofAC[b8][0][1], 65 * sizeof(int));

    stx = 8 * (b8 & 1);
    sty = 8 * (b8 / 2);

    for (y = 0; y < 8; y++) {
        for (x = 0; x < 8; x++) {
            mpr8[y][x] = img->mpr[sty + y][stx + x];
			rec8[y][x] = imgY[(img->pix_y + sty + y)*(img->iStride) + img->pix_x + stx + x];
        }
    }


    // try 4x4
    currMB->trans_split[b8] = 1;
    for (b4 = 0; b4 < 4; b4++) {
        if (Mode_Decision_for_IVC_IntraBlocks(b8, b4, lambda, &cost_tmp, 4)) {
            cbp_4 |= 1 << (b8 * 4 + b4);
        }
        cost_4 += cost_tmp + (int)floor(2.0 * lambda + 0.4999);
    }


    if (cost_8 < cost_4) {
        currMB->trans_split[b8] = 0;
        memcpy(&currMB->intra_pred_modes[b8 * 4], ipred8, sizeof(int) * 4);
        memcpy(img->cofAC[b8][0][0], coef8[0], 65 * sizeof(int));
        memcpy(img->cofAC[b8][0][1], coef8[1], 65 * sizeof(int));

        for (y = 0; y < 8; y++) {
            for (x = 0; x < 8; x++) {
                img->mpr[sty + y][stx + x] = mpr8[y][x];
				imgY[(img->pix_y + sty + y)*(img->iStride) + img->pix_x + stx + x] = rec8[y][x];
            }
        }

        *cost = cost_8;
        return cbp_8;
    } else {
        *cost = cost_4;
        return cbp_4;
    }
}

int Mode_Decision_for_16x16IntraBlocks(double lambda, int *cost)
{
    int  cbp = 0;
    int  cost8x8;

    if (Mode_Decision_for_IVC_IntraBlocks(0, 0, lambda, &cost8x8, 16)) {
        cbp = 0xFFFF;
    }

    *cost = cost8x8 + (int)floor(2.0 * lambda + 0.4999);

    return cbp;
}

/*
*************************************************************************
* Function:8x8 Intra mode decision for an macroblock
* Input:
* Output:
* Return: 
* Attention:
*************************************************************************
*/

int IntraLumaPrediction16x16(double lambda, int* cost)
{
    int cbp = 0, cbp16 = 0;
    int b8, cost8x8, cost16x16;
    int bak_coef[2][COF_SIZE_LUMA];
    int bak_mpr[16][16];
    int bak_ipredmode[16];
    byte bak_rec[16][16];
    int pic_block_x = img->pix_x >> 3;
    int pic_block_y = img->pix_y >> 3;
    int x, y;
    int b4_x = pic_block_x * 2;
    int b4_y = pic_block_y * 2;
    Macroblock *currMB = &img->mb_data[img->current_mb_nr];

    currMB->trans_type = TRANS_NxN;
    *cost = 0;

    for (b8 = 0; b8 < 4; b8++)  {
        cbp |= Mode_Decision_for_8x8IntraBlocks(b8, lambda, &cost8x8);
        *cost += cost8x8;
    }

    if (input->abt_enable) {
        // backup 8x8 encode informations
        memcpy(bak_ipredmode, currMB->intra_pred_modes, sizeof(bak_ipredmode));
        memcpy(bak_coef[0], img->cofAC[0][0][0], COF_SIZE_LUMA * sizeof(int));
        memcpy(bak_coef[1], img->cofAC[0][0][1], COF_SIZE_LUMA * sizeof(int));
        memcpy(bak_mpr, img->mpr, sizeof(bak_mpr));

        for (y = 0; y < 16; y++) {
            for (x = 0; x<16; x++) {
				bak_rec[y][x] = imgY[(img->pix_y + y)*(img->iStride) + img->pix_x + x];
            }
        }

        currMB->trans_type = TRANS_2Nx2N;
        cbp16 = Mode_Decision_for_16x16IntraBlocks(lambda, &cost16x16);

        if (cost16x16 > *cost) {
            currMB->trans_type = TRANS_NxN;
            memcpy(img->cofAC[0][0][0], bak_coef[0], COF_SIZE_LUMA * sizeof(int));
            memcpy(img->cofAC[0][0][1], bak_coef[1], COF_SIZE_LUMA * sizeof(int));
            memcpy(img->mpr, bak_mpr, sizeof(bak_mpr));

            for (y = 0; y < 16; y++) {
                for (x = 0; x < 16; x++) {
					imgY[(img->pix_y + y)*(img->iStride) + img->pix_x + x] = bak_rec[y][x];
                }
            }
            memcpy(currMB->intra_pred_modes, bak_ipredmode, sizeof(bak_ipredmode)); 
        } else { 
            cbp = cbp16;
            *cost = cost16x16;
        }
    }

    return cbp;
}

/*
*************************************************************************
* Function:R-D Cost for an 8x8 Partition
* Input:
* Output:
* Return: 
* Attention:
*************************************************************************
*/

double RDCost_for_8x8blocks (int*    cnt_nonz,   // --> number of nonzero coefficients
                             double  lambda,     // <-- lagrange multiplier
                             int     block,      // <-- 8x8 block number
                             int     mode,       // <-- partitioning mode
                             int     pdir,       // <-- prediction direction
                             int     ref,        // <-- reference frame
                             int     bwd_ref)   // <-- abp type
{
  int  i, j;
  int  rate=0, distortion=0;
  int  dummy, mrate;
  int  fw_mode, bw_mode;
  int  cbp     = 0;
  int  pax     = 8*(block%2);
  int  pay     = 8*(block/2);
  int  i0      = pax/8;
  int  j0      = pay/8;
  int  bframe  = (img->type==B_IMG);
  int  direct  = (bframe && mode==0);
  int  b8value = B8Mode2Value (mode, pdir);

  Macroblock    *currMB    = &img->mb_data[img->current_mb_nr];
  SyntaxElement *currSE    = &img->MB_SyntaxElements[currMB->currSEnr];
  int  **frefarr = refFrArr; 
  int  block_y = img->block_y;
  int  block_x = img->block_x;
  int pix_y = img->pix_y;
  int pix_x = img->pix_x;
  uchar_t *imgY_original  =  imgY_org;
  int  incr_y=1,off_y=0;

  //=====  GET COEFFICIENTS, RECONSTRUCTIONS, CBP
  if (direct)
  { 
    *cnt_nonz = LumaResidualCoding8x8 (&cbp, block, 0, 0, 
      max(0,frefarr[block_y+j0][block_x+i0]), 0
#ifdef RDO_Q
	  , lambda
#endif
	  );      
  }
  else
  {
    fw_mode   = (pdir==0||pdir==2 ? mode : 0);
    bw_mode   = (pdir==1||pdir==2 ? mode : 0);
    *cnt_nonz = LumaResidualCoding8x8 (&cbp, block, fw_mode, bw_mode, ref, bwd_ref
#ifdef RDO_Q
		, lambda
#endif
		);
  }

  for (j = 0; j < 8; j++){
	  for (i = pax; i < pax + 8; i++)
	  {
		  distortion += img->quad[imgY_original[(pix_y + pay +/*j*/incr_y*j + off_y)*(img->width) + pix_x + i] - imgY[(img->pix_y + pay +/*j*/incr_y*j + off_y)*(img->iStride) + img->pix_x + i]];
	  }
  }
    //=====   GET RATE
    //----- block 8x8 mode -----
    ue_linfo (b8value, dummy, &mrate, &dummy);
    rate += mrate;

    //----- motion information -----
    if (!direct)
    {	
      if (pdir==0 || pdir==2)
      {
        rate  += writeMotionVector8x8 (i0, j0, i0+1, j0+1, ref, 0, 1, mode);
      }
      if (pdir==1 || pdir==2)
      {
        rate  += writeMotionVector8x8 (i0, j0, i0+1, j0+1, bwd_ref, 0, 0, mode);
      }
    }

    //----- luminance coefficients -----
    if (*cnt_nonz)
    {
      currMB->cbp = cbp; 
      rate += writeLumaCoeff8x8 (block, 0, 0);

    }

    return (double)distortion + lambda * (double)rate;
}

/*
*************************************************************************
* Function:Sets modes and reference frames for an macroblock
* Input:
* Output:
* Return: 
* Attention:
*************************************************************************
*/


void SetModesAndRefframeForBlocks(int mode)
{
    int i, j, k;
    Macroblock *currMB = &img->mb_data[img->current_mb_nr];
    int  bframe = (img->type == B_IMG);
    int  **fwrefarr = fw_refFrArr;
    int  **bwrefarr = bw_refFrArr;
    int  **frefarr = refFrArr;
    int  block_y = img->block_y;
    int  block_x = img->block_x;


    //--- macroblock type ---
    currMB->mb_type = mode;

    for (i = 0; i < 4; i++) {
        currMB->trans_split[i] = 0;
    }

    //--- block 8x8 mode and prediction direction ---
    switch (mode)
    {
    case 0:
        for (i = 0; i < 4; i++)
        {
            currMB->b8mode[i] = 0;
            currMB->b8pdir[i] = (bframe ? 2 : 0);
            currMB->fw_ref[i] = -1;
        }
        break;
    case 1:
    case 2:
    case 3:
        for (i = 0; i < 4; i++)
        {
            currMB->b8mode[i] = mode;
            currMB->b8pdir[i] = best8x8pdir[mode][i];
            currMB->fw_ref[i] = best8x8ref[mode][i];
        }
        break;
        //case 4:
    case P8x8:
        for (i = 0; i < 4; i++)
        {
            currMB->b8mode[i] = best8x8mode[i];
            currMB->b8pdir[i] = best8x8pdir[mode][i];
            currMB->fw_ref[i] = best8x8ref[mode][i];
        }
        break;
    case I_MB:
        for (i = 0; i < 4; i++)
        {
            currMB->b8mode[i] = IBLOCK;
            currMB->b8pdir[i] = -1;
            currMB->fw_ref[i] = -1;
        }
        break;
    default:
        printf("Unsupported mode in SetModesAndRefframeForBlocks!\n");
        exit(1);
    }

    //--- reference frame arrays ---
    if (mode == 0 || mode == I_MB)
    {
        if (bframe)
        {
            for (j = 0 / 2; j < 2; j++)
            for (i = 0; i < 2; i++)
            {
                k = 2 * j + i;
                if (!mode)
                {
                    fwrefarr[(block_y >> 1) + j][(block_x >> 1) + i] = 0;
                    bwrefarr[(block_y >> 1) + j][(block_x >> 1) + i] = 0;
                }
                else
                {
                    fwrefarr[(block_y >> 1) + j][(block_x >> 1) + i] = -1;
                    bwrefarr[(block_y >> 1) + j][(block_x >> 1) + i] = -1;
                }
            }
        }
        else
        {
            for (j = 0 / 2; j < 2; j++)
            for (i = 0; i < 2; i++)
            {
                frefarr[(block_y >> 1) + j][(block_x >> 1) + i] = (mode == 0 ? 0 : -1);
            }
        }
    }
    else
    {
        if (bframe)
        {
            for (j = 0 / 2; j < 2; j++)
            for (i = 0; i < 2; i++)
            {
                k = 2 * j + i;

                if ((mode == P8x8) && (best8x8mode[k] == 0))
                {

                    fwrefarr[(block_y >> 1) + j][(block_x >> 1) + i] = 0;
                    bwrefarr[(block_y >> 1) + j][(block_x >> 1) + i] = 0;

                }
                else
                {
                    if (IS_FW&&IS_BW)
                    {
                        fwrefarr[(block_y >> 1) + j][(block_x >> 1) + i] = best8x8symref[mode][k][0];
                        bwrefarr[(block_y >> 1) + j][(block_x >> 1) + i] = best8x8symref[mode][k][1];
                    }
                    else
                    {
                        fwrefarr[(block_y >> 1) + j][(block_x >> 1) + i] = (IS_FW ? best8x8ref[mode][k] : -1);
                        bwrefarr[(block_y >> 1) + j][(block_x >> 1) + i] = (IS_BW ? best8x8bwref[mode][k] : -1);
                    }
                }
            }
        }
        else
        {
            for (j = 0 / 2; j < 2; j++)
            for (i = 0; i < 2; i++)
            {
                k = 2 * j + i;
                frefarr[(block_y >> 1) + j][(block_x >> 1) + i] = (IS_FW ? best8x8ref[mode][k] : -1);
            }
        }
    }
}

/*
*************************************************************************
* Function:Sets motion vectors for an macroblock
* Input:
* Output:
* Return: 
* Attention:
*************************************************************************
*/

void SetMotionVectorsMB (Macroblock* currMB, int bframe)
{
  int i, j, k, mode8, pdir8, ref, by, bx, bxr, dref;
  int ***mv        = tmp_mv;
  int *****all_bimv= img->all_bimv;
  int ***** p_biMV = img->bimv;
  int *****all_mv  = img->all_mv;
  int *****all_bmv = img->all_bmv;
  int ***fwMV      = tmp_fwMV;
  int ***bwMV      = tmp_bwMV;
  int *****imgmv   = img->mv;
  int *****p_fwMV  = img->p_fwMV;
  int *****p_bwMV  = img->p_bwMV;
  int **refar      = refFrArr;
  int **frefar     = fw_refFrArr;
  int block_y      = img->block_y>>1;
  int block_x      = img->block_x>>1;
  int **brefar     = bw_refFrArr;
  int  bw_ref;

  for (j=0/2;j<2;j++)
    for (i=0; i<2; i++)
    {
      mode8 = currMB->b8mode[k=2*j+i];
      pdir8 = currMB->b8pdir[k];

	  if(pdir8 == 2 && mode8 != 0) 
	  {
		  all_mv = img->all_omv;
		  p_fwMV = img->omv;
	  }
	  else if(pdir8 == 3)
	  {
		  all_mv = img->all_Lbimv;
		  imgmv = img->Lbimv;
	  }
	  else
	  {
		  all_mv = img->all_mv;
		  imgmv = img->mv;
	  }

      by    = block_y+j;
      bxr   = block_x+i;
      bx    = block_x+i+4;
      ref   = (bframe?frefar:refar)[by][bxr];
      bw_ref = (bframe?brefar:refar)[by][bxr];

      if (!bframe)
      {
        if (mode8!=IBLOCK && ref != -1)
        {
          mv   [0][by][bx] = all_mv [i][j][ ref][mode8][0];
          mv   [1][by][bx] = all_mv [i][j][ ref][mode8][1];
        }
        else
        {
          mv   [0][by][bx] = 0;
          mv   [1][by][bx] = 0;
        }
      }
      else
      {
        if (pdir8==-1) // intra
        {
          fwMV [0][by][bx]     = 0;
          fwMV [1][by][bx]     = 0;
          bwMV [0][by][bx]     = 0;
          bwMV [1][by][bx]     = 0;
          dfMV     [0][by][bx] = 0;
          dfMV     [1][by][bx] = 0;
          dbMV     [0][by][bx] = 0;
          dbMV     [1][by][bx] = 0;
        }
        else if (pdir8==0) // forward
        {
          fwMV [0][by][bx]     = all_mv [i][j][ ref][mode8][0];
          fwMV [1][by][bx]     = all_mv [i][j][ ref][mode8][1];
          bwMV [0][by][bx]     = 0;
          bwMV [1][by][bx]     = 0;
          dfMV     [0][by][bx] = 0;
          dfMV     [1][by][bx] = 0;
          dbMV     [0][by][bx] = 0;
          dbMV     [1][by][bx] = 0;
        }
        else if (pdir8==1) // backward
        {
          fwMV [0][by][bx] = 0;
          fwMV [1][by][bx] = 0;
          {
            bwMV [0][by][bx] = all_bmv[i][j][   bw_ref][mode8][0];/*lgp*13*/
            bwMV [1][by][bx] = all_bmv[i][j][   bw_ref][mode8][1];/*lgp*13*/
          }
          dfMV     [0][by][bx] = 0;
          dfMV     [1][by][bx] = 0;
          dbMV     [0][by][bx] = 0;
          dbMV     [1][by][bx] = 0;
        }
        else if (mode8!=0) // bidirect
        {
          fwMV [0][by][bx] = all_mv [i][j][ ref][mode8][0];
          fwMV [1][by][bx] = all_mv [i][j][ ref][mode8][1];
          {
            int delta_P,TRp,DistanceIndexFw,DistanceIndexBw,refframe,delta_PB;
            refframe = ref;
            delta_P = 2*(img->imgtr_next_P_frm - img->imgtr_last_P_frm);
            delta_P = (delta_P + 512)%512;  

              TRp = (refframe+1)*delta_P;  //the lates backward reference
            
            delta_PB = 2*(picture_distance - img->imgtr_last_P_frm);		
            TRp = (TRp + 512)%512;
            delta_PB = (delta_PB + 512)%512; 
            
              DistanceIndexFw = delta_PB;
  
            DistanceIndexBw    = (TRp - DistanceIndexFw+512)%512;


            bwMV [0][by][bx] = - ((all_mv[i][j][ref][mode8][0]*DistanceIndexBw*(512/DistanceIndexFw)+256)>>9); 
            bwMV [1][by][bx] = - ((all_mv[i][j][ref][mode8][1]*DistanceIndexBw*(512/DistanceIndexFw)+256)>>9); 
          }

          dfMV     [0][by][bx] = 0;
          dfMV     [1][by][bx] = 0;
          dbMV     [0][by][bx] = 0;
          dbMV     [1][by][bx] = 0;
        }
        else // direct
        {
         
            dref = 0;
             
            fwMV [0][by][bx] = dfMV     [0][by][bx] = all_mv [i][j][dref][    0][0];
            fwMV [1][by][bx] = dfMV     [1][by][bx] = all_mv [i][j][dref][    0][1];

            bwMV [0][by][bx] = dbMV     [0][by][bx] = all_bmv[i][j][   0][    0][0];
            bwMV [1][by][bx] = dbMV     [1][by][bx] = all_bmv[i][j][   0][    0][1];

        }
      }
    }
}
/*
*************************************************************************
* Function:Store macroblock parameters
* Input:
* Output:
* Return: 
* Attention:
*************************************************************************
*/

void store_macroblock_parameters (int mode)
{
    int  i, j;
    Macroblock *currMB = &img->mb_data[img->current_mb_nr];
    int        bframe = (img->type == B_IMG);
    int        **frefar = ((img->type == B_IMG) ? fw_refFrArr : refFrArr);
    int        **brefar = bw_refFrArr;
    int    block_x = img->block_x;
    int    pix_x = img->pix_x;
    int    pix_c_x = img->pix_c_x;
    int    block_y = img->block_y;
    int    pix_y = img->pix_y;
    int    pix_c_y = img->pix_c_y;


    //--- store best mode ---
    best_mode = mode;
    best_trans = currMB->trans_type;
    best_c_imode = currMB->c_ipred_mode;
    memcpy(best_splite, currMB->trans_split, sizeof(best_splite));

    for (i =/*0*/0; i < 4; i++) {
        b8mode[i] = currMB->b8mode[i];
        b8pdir[i] = currMB->b8pdir[i];
    }

    //--- reconstructed blocks ----
    for (j = 0; j < 16; j++) {
        for (i = 0; i < 16; i++) {
			rec_mbY[j][i] = imgY[(img->pix_y + j)*(img->iStride) + img->pix_x + i];
        }
    }

    for (j = 0; j < 8; j++) {
        for (i = 0; i < 8; i++) {
			rec_mbU[j][i] = imgU[(img->pix_c_y + j)*(img->iStrideC) + img->pix_c_x + i];
			rec_mbV[j][i] = imgV[(img->pix_c_y + j)*(img->iStrideC) + img->pix_c_x + i];
        }
    }


    if (best_mode == I_MB) {
        int b4_x = img->block8_x * 2;
        int b4_y = img->block8_y * 2;
        for (i = 0; i < 16; i++) {
            best_intra_pred_modes_tmp[i] = currMB->intra_pred_modes[i];
        }
        for (j = 0; j < 16; j++) {
            for (i = 0; i < 16; i++) {
                best_mpr_tmp[i][j] = img->mpr[i][j];
            }
        }
    }


    //--- coeff, cbp, kac ---
    if (mode || bframe) {
        swap_cofAC(cofAC_MD, img->cofAC);
        cbp = currMB->cbp;
    } else {
        cbp = 0;
    }

    for (j = 0 / 2; j < 2; j++) {
        for (i = 0; i < 2; i++) {
            frefframe[j][i] = frefar[(block_y >> 1) + j][(block_x >> 1) + i];
            if (bframe) {
                brefframe[j][i] = brefar[(block_y >> 1) + j][(block_x >> 1) + i];
            }
        }
    }
}

/*
*************************************************************************
* Function:R-D Cost for a macroblock
* Input:
* Output:
* Return: 
* Attention:
*************************************************************************
*/

int RDCost_for_macroblocks(double   lambda,      // <-- lagrange multiplier
		  				   int      mode,        // <-- modus (0-COPY/DIRECT, 1-16x16, 2-16x8, 3-8x16, 4-8x8(+), 5-Intra4x4, 6-Intra8x8)
						   double*  min_rdcost)  // <-> minimum rate-distortion cost
{
    int        i, j;
    int         rate = 0, distortion = 0;
    double      rdcost;
    Macroblock  *currMB = &img->mb_data[img->current_mb_nr];
    int         bframe = (img->type == B_IMG);
    int          dummy;
    uchar_t      *imgY_orig = imgY_org;
    uchar_t      *imgU_orig = imgU_org;
	uchar_t      *imgV_orig = imgV_org;
    int         pix_y = img->pix_y;
    int         pix_c_y = img->pix_c_y;
    int         pix_x = img->pix_x;
    int         pix_c_x = img->pix_c_x;

    int         rate_tmp, rate_top = 0, rate_bot = 0, distortion_blk, distortion_top = 0, distortion_bot = 0;
    double      rdcost_top, rdcost_bot;
    int         incr_y = 1, off_y = 0;
    int         block8x8, block_x, block_y;
    int         c_distortion = 0, lum_bits;
    int tmp_cbp;
    int mb_available_up;
    int mb_available_left;
    int mb_available_up_left;
    int k;
    int coef_num = 64;
    int tmp_cbp_luma;

    currMB->trans_type = TRANS_NxN;
    mv_out_of_range = 1;

    if (mode == 1 && input->abt_enable) {
        currMB->trans_type = TRANS_2Nx2N;
    }

    //=====  SET REFERENCE FRAMES AND BLOCK MODES
    SetModesAndRefframeForBlocks(mode);

    //=====  GET COEFFICIENTS, RECONSTRUCTIONS, CBP
    if (mode == I_MB)
    {
        currMB->cbp = IntraLumaPrediction16x16(lambda, &dummy);
        if (currMB->trans_type == TRANS_2Nx2N) {
            coef_num = 256;
        }
        IntraChromaPrediction8x8(&mb_available_up, &mb_available_left, &mb_available_up_left);
        memcpy(cofAC_IC[0][0][0], img->cofAC[0][0][0], COF_SIZE_LUMA * sizeof(int));
        memcpy(cofAC_IC[0][0][1], img->cofAC[0][0][1], COF_SIZE_LUMA * sizeof(int));
    } else {
        LumaResidualCoding(
#ifdef RDO_Q
            lambda
#endif
            );
    }

    tmp_cbp_luma = currMB->cbp;

    for (currMB->c_ipred_mode = DC_PRED_8; currMB->c_ipred_mode <= PLANE_8; currMB->c_ipred_mode++)//
    {
        if (IS_INTRA(currMB))// bypass if c_ipred_mode is not allowed
        {
            if ((currMB->c_ipred_mode == VERT_PRED_8 && !mb_available_up) ||
                (currMB->c_ipred_mode == HOR_PRED_8 && !mb_available_left) ||
                (currMB->c_ipred_mode == PLANE_8 && (!mb_available_left || !mb_available_up || !mb_available_up_left)))
                continue;
        }
        if (currMB->c_ipred_mode == DC_PRED_8 || (IS_INTRA(currMB))) {
            if (currMB->c_ipred_mode != DC_PRED_8) {
                currMB->cbp = tmp_cbp_luma;
                memcpy(img->cofAC[0][0][0], cofAC_IC[0][0][0], COF_SIZE_LUMA * sizeof(int));
                memcpy(img->cofAC[0][0][1], cofAC_IC[0][0][1], COF_SIZE_LUMA * sizeof(int));
            }
            dummy = 0;
            ChromaResidualCoding(&dummy
#ifdef RDO_Q
                , lambda
#endif
                );

            //=====   GET DISTORTION
            distortion_top = distortion_bot = c_distortion = 0;
            // LUMA
            for (block8x8 = 0; block8x8 < 4; block8x8++) {
                block_y = (block8x8 / 2) * 8;
                block_x = (block8x8 % 2) * 8;

                distortion_blk = 0;

                for (j = 0; j < 8; j++) {
                    for (i = 0; i < 8; i++) {
						distortion_blk += img->quad[imgY_orig[(block_y + incr_y*j + off_y + pix_y)*(img->width) + block_x + i + pix_x] - imgY[(img->pix_y + block_y + incr_y*j + off_y)*(img->iStride) + img->pix_x + block_x + i]];
                    }
                }

                block_y = (block8x8 / 2) * 4;
                block_x = (block8x8 % 2) * 4;

                // CHROMA
                k = 4;
                for (j = 0; j < k; j++) {
                    for (i = 0; i < 4; i++) {
						distortion_blk += img->quad[imgU_orig[(block_y + incr_y*j + off_y + pix_c_y)*(img->width_cr) + block_x + i + pix_c_x] - imgU[(img->pix_c_y + block_y + incr_y*j + off_y)*(img->iStrideC) + img->pix_c_x + block_x + i]];
						distortion_blk += img->quad[imgV_orig[(block_y + incr_y*j + off_y + pix_c_y)*(img->width_cr) + block_x + i + pix_c_x] - imgV[(img->pix_c_y + block_y + incr_y*j + off_y)*(img->iStrideC) + img->pix_c_x + block_x + i]];

						c_distortion += img->quad[imgU_orig[(block_y + incr_y*j + off_y + pix_c_y)*(img->width_cr) + block_x + i + pix_c_x] - imgU[(img->pix_c_y + block_y + incr_y*j + off_y)*(img->iStrideC) + img->pix_c_x + block_x + i]];
						c_distortion += img->quad[imgV_orig[(block_y + incr_y*j + off_y + pix_c_y)*(img->width_cr) + block_x + i + pix_c_x] - imgV[(img->pix_c_y + block_y + incr_y*j + off_y)*(img->iStrideC) + img->pix_c_x + block_x + i]];
                    }
                }

                if (block8x8 < 2)
                    distortion_top += distortion_blk;
                else
                    distortion_bot += distortion_blk;

            }

            //=====   S T O R E   C O D I N G   S T A T E   =====
            store_coding_state(cs_cm);

            //=====   GET RATE
            //----- macroblock header -----
            rate_top = rate_bot = 0;//qyu add
            tmp_cbp = (currMB->cbp != 0);

            rate_top = writeMBHeader(1);
 
            if (mode)  {
                //----- motion information -----
                storeMotionInfo(0);
                writeMVD(0, &rate_top, &rate_bot);
                writeCBPandDqp(0, &rate_top, &rate_bot);
            }

            if (mode || (bframe && tmp_cbp)) {
                for (block8x8 = 0; block8x8 < 6; block8x8++) {
                        rate_tmp = writeBlockCoeff(block8x8);

                        if (block8x8 < 2) {
                            rate_top += rate_tmp;
                        } else if (block8x8 < 4) {
                            rate_bot += rate_tmp;
                        }
                        else if (block8x8 == 4) {
                            rate_top += rate_tmp;
                        }
                         else if (block8x8 == 5) {
                            rate_bot += rate_tmp;
                        }

                        if (block8x8 < 4) {
                            lum_bits = rate_bot + rate_top;
                        }
                }
            }
            //=====   R E S T O R E   C O D I N G   S T A T E   =====
            reset_coding_state(cs_cm);

            rdcost_top = (double)distortion_top + lambda * (double)rate_top;
            rdcost_bot = (double)distortion_bot + lambda * (double)rate_bot;

            rdcost = rdcost_top + rdcost_bot;
        }

        if (mode == I_MB) {
            if (rdcost < *min_rdcost) {
                *min_rdcost = rdcost;
                store_macroblock_parameters(mode);
            }
        }
    }

    if (rdcost >= *min_rdcost) {
        return 0;
    }

    if (!mv_out_of_range) {
        return 0;   //return 0 means it is not the best mode
    }

    //=====   U P D A T E   M I N I M U M   C O S T   =====
    *min_rdcost = rdcost;

    return 1;
}




/*
*************************************************************************
* Function:Set stored macroblock parameters
* Input:
* Output:
* Return: 
* Attention:
*************************************************************************
*/

void set_stored_macroblock_parameters()
{
    int  i, j;
    Macroblock  *currMB = &img->mb_data[img->current_mb_nr];
    int         mode = best_mode;
    int         bframe = (img->type == B_IMG);
    int         **frefar = ((img->type == B_IMG) ? fw_refFrArr : refFrArr);
    int         **brefar = bw_refFrArr;
    int     block_x = img->block_x;
    int     block_y = img->block_y;


    //===== reconstruction values =====
	for (j = 0; j < 16; j++)
	{
		for (i = 0; i < 16; i++)
		{
			imgY[(img->pix_y + j)*(img->iStride) + img->pix_x + i] = rec_mbY[j][i];
		}
	}

	for (j = 0; j < 8; j++)
	{
		for (i = 0; i < 8; i++)
		{
			imgU[(img->pix_c_y + j)*(img->iStrideC) + img->pix_c_x + i] = rec_mbU[j][i];
			imgV[(img->pix_c_y + j)*(img->iStrideC) + img->pix_c_x + i] = rec_mbV[j][i];
		}
	}


    //===== coefficients and cbp =====
    swap_cofAC(cofAC_MD, img->cofAC);
    currMB->cbp = cbp;
    //==== macroblock type ====
    currMB->mb_type = mode;
    currMB->trans_type = best_trans;
    memcpy(currMB->trans_split, best_splite, sizeof(best_splite));

    for (i = 0; i < 4; i++)
    {
        currMB->b8mode[i] = b8mode[i];
        currMB->b8pdir[i] = b8pdir[i];
    }

    //==== reference frames =====
	for (j = 0 / 2; j < 2; j++){
		for (i = 0; i < 2; i++)
		{
			frefar[(block_y >> 1) + j][(block_x >> 1) + i] = frefframe[j][i];
			if (img->type == P_IMG)
				currMB->fw_ref[j * 2 + i] = frefframe[j][i];
		}
	}

    if (bframe)
    {
        for (j = 0 / 2; j < 2; j++)
        for (i = 0; i < 2; i++)
        {
            brefar[(block_y >> 1) + j][(block_x >> 1) + i] = brefframe[j][i];
        }
    }

    if (currMB->mb_type == I_MB)
    {
        for (i = 0; i < 16; i++) 
		{
            currMB->intra_pred_modes[i] = best_intra_pred_modes_tmp[i];
        }

        for (j = 0; j < 16; j++) 
		{
            for (i = 0; i < 16; i++) 
			{
                img->mpr[i][j] = best_mpr_tmp[i][j];
            }
        }
    }

    //==== intra prediction modes ====
    if (currMB->mb_type == I_MB)
        currMB->c_ipred_mode = best_c_imode;
    else
        currMB->c_ipred_mode = DC_PRED_8;

    if (mode != I_MB)
    {
        int b4_x = img->block8_x * 2;
        int b4_y = img->block8_y * 2;
        for (i = 0; i < 16; i++) 
		{
            currMB->intra_pred_modes[i] = DC_PRED;
        }
    }

    //==== motion vectors =====
    SetMotionVectorsMB(currMB, bframe);
    storeMotionInfo(0 / 2);
}

/*
*************************************************************************
* Function:Set reference frames and motion vectors
* Input:
* Output:
* Return: 
* Attention:
*************************************************************************
*/

void SetRefAndMotionVectors (int block, int mode, int ref, int bw_ref,int pdir)/*lgp*13*/
{
  int i, j;
  int     bframe       = (img->type==B_IMG);
  int     pmode        = (mode==1||mode==2||mode==3?mode:4);
  int     j0           = (block/2);
  int     i0           = (block%2);
  int     j1           = j0 + (input->blc_size[pmode][1]>>3);
  int     i1           = i0 + (input->blc_size[pmode][0]>>3);
  int**   frefArr      = (bframe ? fw_refFrArr : refFrArr);
  int**   brefArr      = bw_refFrArr;
  int***  bmvArr       = tmp_bwMV;
  int***** all_bmv_arr = img->all_bmv;
  int   block_x        = img->block_x>>1;
  int   block_y        = img->block_y>>1; 

  int***  fmvArr  = (bframe ? tmp_fwMV    : tmp_mv);
  int***** all_mv_arr = (pdir==2 && mode != 0 /*&&mode ! =IBLOCK*/)?img->all_omv:img->all_mv; 

  if((img->type == P_IMG) && (pdir == 3))
  {
	  all_mv_arr = img->all_Lbimv;
  }

  if ((pdir==0 || pdir==2 || pdir==3) && (mode!=IBLOCK && mode!=0))
  {
    for (j=j0; j<j1; j++)
      for (i=i0; i<i1; i++)
      {
        fmvArr[0][block_y+j][block_x+i+4] = all_mv_arr[i][j][ref][mode][0];
        fmvArr[1][block_y+j][block_x+i+4] = all_mv_arr[i][j][ref][mode][1];
        frefArr  [block_y+j][block_x+i  ] = ref;
      }
  }
  else
  {
    if(!mode&&bframe)
    {
      for (j=j0; j<j0+1; j++)
        for (i=i0; i<i0+1; i++)
        {
            ref = 0;
            fmvArr[0][block_y+j][block_x+i+4] = all_mv_arr[i][j][ref][mode][0];
            fmvArr[1][block_y+j][block_x+i+4] = all_mv_arr[i][j][ref][mode][1];
            frefArr  [block_y+j][block_x+i  ] = ref;

        }
    }
    else    
      for (j=j0; j<j0+1; j++)
        for (i=i0; i<i0+1; i++)
        {
          fmvArr[0][block_y+j][block_x+i+4] = 0;
          fmvArr[1][block_y+j][block_x+i+4] = 0;
          frefArr  [block_y+j][block_x+i  ] = -1;
        }
  }

  if ((pdir==1 || pdir==2) && (mode!=IBLOCK && mode!=0))
  {
    for (j=j0; j<j0+1; j++)
      for (i=i0; i<i0+1; i++)
      {
        if(pdir == 2)
        {
          {
            int delta_P,TRp,DistanceIndexFw,DistanceIndexBw,refframe,delta_PB;	
            refframe = ref;
            delta_P = 2*(img->imgtr_next_P_frm - img->imgtr_last_P_frm);
            delta_P = (delta_P + 512)%512;   

              TRp = (refframe+1)*delta_P;  //the lates backward reference

            delta_PB = 2*(picture_distance - img->imgtr_last_P_frm);		
            TRp = (TRp + 512)%512;
            delta_PB = (delta_PB + 512)%512;  
       
              DistanceIndexFw = delta_PB;
  	 	  
            DistanceIndexBw    = (TRp - DistanceIndexFw+512)%512;

            bmvArr[0][block_y+j][block_x+i+4] = - ((all_mv_arr[i][j][ref][mode][0]*DistanceIndexBw*(512/DistanceIndexFw)+256)>>9);
            bmvArr[1][block_y+j][block_x+i+4] = - ((all_mv_arr[i][j][ref][mode][1]*DistanceIndexBw*(512/DistanceIndexFw)+256)>>9);
          }				
        }
        else
        {
          bmvArr[0][block_y+j][block_x+i+4] = all_bmv_arr[i][j][bw_ref][mode][0];
          bmvArr[1][block_y+j][block_x+i+4] = all_bmv_arr[i][j][bw_ref][mode][1];
        }
        brefArr  [block_y+j][block_x+i  ] = bw_ref;
      }
  }
  else if (bframe)
  {
    if(!mode)
    {
      for (j=j0; j<j0+1; j++)
        for (i=i0; i<i0+1; i++)
        {

            
              bmvArr[0][block_y+j][block_x+i+4] = all_bmv_arr[i][j][0][mode][0];
              bmvArr[1][block_y+j][block_x+i+4] = all_bmv_arr[i][j][0][mode][1];
              ref = refFrArr[block_y+j][block_x+i];             

                brefArr  [block_y+j][block_x+i  ] = 0;// min(ref,0); 



        }   
    }
    else
    {
      for (j=j0; j<j0+1; j++)
        for (i=i0; i<i0+1; i++)
        {
          bmvArr[0][block_y+j][block_x+i+4] = 0;
          bmvArr[1][block_y+j][block_x+i+4] = 0;
          brefArr  [block_y+j][block_x+i  ] = -1;
        }
    }
  }
}

void SetRef(int *adjust_ref, int* max_ref)
{  
	*adjust_ref = (img->type == B_IMG ? 1 : 0);
	*adjust_ref = min(*adjust_ref, *max_ref - 1);

	*max_ref = min(img->nb_references, img->buf_cycle);
	*adjust_ref = 0;

	if (*max_ref > 1 && img->type == B_IMG)
	{
		*max_ref = 1;
	}
}

void ME(int mode,int bframe, int max_ref, int adjust_ref, double lambda_motion, int write_ref, int lambda_motion_factor, int* valid,    double lambda_mode,Macroblock* currMB)
{   
	int scale_refframe=0;
	int fw_mcost, bw_mcost, bid_mcost,max_mcost=(1<<30);
	int p_bid_mcost;
    int p_bid_ref;

	double  min_rdcost,rdcost;

	int best_fw_ref=0,best_bw_ref=0,bid_best_fw_ref=0,bid_best_bw_ref=0,best_pdir=0;
	int block;
	int index;
	int block_x   = img->block_x;
	int block_y   = img->block_y;
	int cnt_nonz=0,best_cnt_nonz=0;
	static const int  b8_mode_table[2]  = {0, 4};         // DO NOT CHANGE ORDER !!!	
	//===== set direct motion vectors =====

	if(mode==8 )
		mode=4;

	//===== MOTION ESTIMATION FOR 16x16 BLOCKS =====
	if(mode>0&&mode<4)
	{
		if (valid[mode])
		{
			for (  block=0/2; block<(mode==1?1:2); block++)
			{		
				ForwardPred( &fw_mcost,& best_fw_ref, &best_bw_ref,  max_ref,  adjust_ref, mode,  block,   lambda_motion,  write_ref,lambda_motion_factor);
				if(img->type == P_IMG && input->multiple_hp_flag )
					MHMC_ForwardPred(&p_bid_mcost,&p_bid_ref, &best_bw_ref, max_ref, adjust_ref, mode,  block,   lambda_motion,  write_ref,lambda_motion_factor);

				if (bframe)
				{
					BiPred(&best_bw_ref, &bw_mcost, &bid_mcost, &bid_best_fw_ref, &bid_best_bw_ref, mode, block, lambda_motion, max_ref, adjust_ref);

					//--- get prediction direction ----
					if (fw_mcost <= bw_mcost && fw_mcost <= bid_mcost)
					{
						best_pdir = 0;
						best_bw_ref = 0;

					}
					else if (bw_mcost <= fw_mcost && bw_mcost <= bid_mcost)
					{
						best_pdir = 1;

						best_fw_ref = 0;
					}
					else
					{
						best_pdir = 2;

					}
				}
				else // if (bframe)
				{
					if(input->multiple_hp_flag)
					{
						if(fw_mcost <= p_bid_mcost)
							best_pdir = 0;
						else
						{
							best_pdir = 3;
							best_fw_ref = p_bid_ref;
						}
					}
					else
						best_pdir = 0;
				}

				//----- set reference frame and direction parameters -----
				if (mode==3)
				{
					best8x8ref [3][  block] = best8x8ref [3][  block+2] = best_fw_ref;
					best8x8pdir[3][  block] = best8x8pdir[3][  block+2] = best_pdir;
					best8x8bwref   [3][block] = best8x8bwref   [3][block+2] = best_bw_ref;			
					best8x8symref  [3][block][0] = best8x8symref   [3][block+2][0] = bid_best_fw_ref;			
					best8x8symref  [3][block][1] = best8x8symref   [3][block+2][1] = bid_best_bw_ref;
				}
				else if (mode==2)
				{
					best8x8ref [2][2*block] = best8x8ref [2][2*block+1] = best_fw_ref;
					best8x8pdir[2][2*block] = best8x8pdir[2][2*block+1] = best_pdir;
					best8x8bwref   [2][2*block] = best8x8bwref   [2][2*block+1] = best_bw_ref;			
					best8x8symref   [2][2*block][0] = best8x8symref   [2][2*block+1][0] = bid_best_fw_ref;
					best8x8symref   [2][2*block][1] = best8x8symref   [2][2*block+1][1] = bid_best_bw_ref;			
				}
				else
				{
					best8x8ref [1][0] = best8x8ref [1][1] = best8x8ref [1][2] = best8x8ref [1][3] = best_fw_ref;
					best8x8pdir[1][0] = best8x8pdir[1][1] = best8x8pdir[1][2] = best8x8pdir[1][3] = best_pdir;
					best8x8bwref   [1][0] = best8x8bwref   [1][1] = best8x8bwref   [1][2] = best8x8bwref   [1][3] = best_bw_ref;			
					best8x8symref [1][0][0] = best8x8symref [1][1][0] = best8x8symref [1][2][0] = best8x8symref [1][3][0] = bid_best_fw_ref;
					best8x8symref [1][0][1] = best8x8symref [1][1][1] = best8x8symref [1][2][1] = best8x8symref [1][3][1] = bid_best_bw_ref;
				}

				//--- set reference frames and motion vectors ---
				if (mode > 1 && block == 0)
				{
					SetRefAndMotionVectors(block, mode, best_pdir == 2 ? bid_best_fw_ref : best_fw_ref,
										   best_pdir == 2 ? bid_best_bw_ref : best_bw_ref, best_pdir);
				}
			}

		} // if (valid[mode])
	} // for (mode=3; mode>0; mode--)*/


	if ((mode==4)&&(valid[0] || valid[4])) 
	{
		//cost8x8 = 0;  

		//===== store coding state of macroblock =====
		store_coding_state (cs_mb);

		//=====  LOOP OVER 8x8 SUB-PARTITIONS  (Motion Estimation & Mode Decision) =====
		for (cbp8x8=cnt_nonz_8x8=0, block=0; block<4; block++)
		{
			//--- set coordinates ---

			//=====  LOOP OVER POSSIBLE CODING MODES FOR 8x8 SUB-PARTITION  =====
			for (min_rdcost=1e30,	 index=(bframe?0:1); index<2; index++) 
			{
				if (valid[mode=b8_mode_table[index]])
				{
					if (mode==0)
					{
						best_fw_ref = -1;
						best_pdir   =  2;
					} // if (mode==0)
					else
					{
						ForwardPred( &fw_mcost,& best_fw_ref, &best_bw_ref,  max_ref,  adjust_ref, mode,  block,   lambda_motion,  write_ref,lambda_motion_factor);
						if(img->type == P_IMG && input->multiple_hp_flag)
							MHMC_ForwardPred(&p_bid_mcost,&p_bid_ref, &best_bw_ref, max_ref, adjust_ref, mode,  block,   lambda_motion,  write_ref,lambda_motion_factor);

						if (bframe)
						{
							BiPred( &best_bw_ref, &bw_mcost, &bid_mcost, &bid_best_fw_ref, &bid_best_bw_ref, mode,  block,  lambda_motion, max_ref,  adjust_ref );
							//--- get prediction direction ----
							if(fw_mcost<bw_mcost && fw_mcost<bid_mcost)
							{
								best_pdir = 0;

								best_bw_ref = 0;
							}
							else if (bw_mcost<fw_mcost && bw_mcost<bid_mcost)
							{
								best_pdir = 1;

								best_fw_ref = 0;
							}
							else
							{
								best_pdir = 2;

							}
						}
						else // if (bframe)
						{
							if(input->multiple_hp_flag)
							{
								if(fw_mcost <= p_bid_mcost)
									best_pdir = 0;
								else
								{
									best_pdir = 3;
									best_fw_ref = p_bid_ref;
								}
							}
							else
								best_pdir = 0;
						}
					} // if (mode1!=0)

					//--- store coding state before coding with current mode1 ---
					store_coding_state (cs_cm);

					if (bframe)
						rdcost = RDCost_for_8x8blocks (&cnt_nonz, lambda_mode,block, mode, best_pdir,
						best_pdir==2?bid_best_fw_ref:best_fw_ref,best_pdir==2?bid_best_bw_ref:best_bw_ref);
					else
						rdcost = 1<<29;

					if (!img->mv_range_flag &&!mode && input->rdopt) {
						rdcost = (1<<30);
						img->mv_range_flag = 1;
					}

					//--- set variables if best mode1 has changed ---
					if (( input->rdopt && rdcost < min_rdcost)	 )
					{

						min_rdcost                 = rdcost;
						best8x8mode        [block] = mode;
						best8x8pdir  [P8x8][block] = best_pdir;
						best8x8ref   [P8x8][block] = best_fw_ref;
						best8x8bwref   [P8x8][block] = best_bw_ref;
						best8x8symref[P8x8][block][0] = bid_best_fw_ref;
						best8x8symref[P8x8][block][1] = bid_best_bw_ref;
						//--- store number of nonzero coefficients ---
						best_cnt_nonz  = cnt_nonz;

						//--- store coding state ---
						store_coding_state (cs_b8);

					} // if (rdcost <= min_rdcost)

					//--- re-set coding state as it was before coding with current mode1 was performed ---
					reset_coding_state (cs_cm);
				} // if (valid[mode1=b8_mode_table[index]])
			} // for (min_rdcost=1e30, index=(bframe?0:1); index<6; index++)

			//----- set cbp and count of nonzero coefficients ---
			if (best_cnt_nonz)
			{
				cbp8x8        |= (1<<block);
				cnt_nonz_8x8  += best_cnt_nonz;
			}

            mode = best8x8mode[block];
            //===== reset intra prediction modes (needed for prediction, must be stored after 8x8 mode dec.) =====

            if (block < 3)
            {
                //===== set motion vectors and reference frames (prediction) =====			
                SetRefAndMotionVectors(block, mode, best8x8pdir[P8x8][block] == 2 ? best8x8symref[P8x8][block][0] :
                    best8x8ref[P8x8][block], best8x8pdir[P8x8][block] == 2 ? best8x8symref[P8x8][block][1] :
                    best8x8bwref[P8x8][block], best8x8pdir[P8x8][block]);
            } // if (block<3)

            //===== set the coding state after current block =====
            reset_coding_state(cs_b8);
        } // for (cbp8x8=cnt_nonz_8x8=0, block=0; block<4; block++)

        //--- re-set coding state (as it was before 8x8 block coding) ---
        reset_coding_state(cs_mb);
    }
}

/*
*************************************************************************
* Function:Mode Decision for a macroblock
* Input:
* Output:
* Return: 
* Attention:
*************************************************************************
*/
void encode_one_macroblock()
{
    static const int  b8_mode_table[2] = { 0, 4 };
    static const int  mb_mode_table[7] = { 0, 1, 2, 3, P8x8, I_MB };

    int         valid[MAXMODE];
    int         index, mode, ctr16x16;
    double      qp, lambda_mode, lambda_motion, min_rdcost, rdcost = 0, max_rdcost = 1e30;
    int         lambda_motion_factor;
    int         intra = img->type == I_IMG;
    int         bframe = img->type == B_IMG;
    int         write_ref = input->no_multpred > 1;
    int         max_ref = img->nb_references;
    int         adjust_ref;
    Macroblock* currMB = &img->mb_data[img->current_mb_nr];

    SetRef(&adjust_ref, &max_ref);
    img->mv_range_flag = 1;

	if (input->usefme)
	{
		decide_intrabk_SAD();
	}

    //===== SET VALID MODES =====
    valid[I_MB] = 1;//intra_8x8
    valid[0] = (!intra);//skip
    valid[1] = (!intra && input->InterSearch16x16);//16x16
    valid[2] = (!intra && input->InterSearch16x8);
    valid[3] = (!intra && input->InterSearch8x16);
    valid[4] = (!intra && input->InterSearch8x8);//8x8
    valid[5] = 0;
    valid[6] = 0;
    valid[7] = 0;
    valid[P8x8] = (valid[4] || valid[5] || valid[6] || valid[7]);//sub_8x8

    //===== SET LAGRANGE PARAMETERS =====
    if (input->rdopt)
    {
#if LAMBDA_MODE_OPT
        qp = (double)img->qp - SHIFT_QP;
#else	  
        qp = (double)img->qp - SHIFT_QP;
#endif

        if (input->successive_Bframe > 0)
        {
#if LAMBDA_MODE_OPT
            lambda_mode = 0.68 * pow (2, qp/4.0) * (img->type==B_IMG? max(2.00,min(4.00,(qp / 8.0))):1.0); 
#else  
            lambda_mode = 0.68 * pow(2, qp / 4.0) * (img->type == B_IMG ? max(2.00, min(4.00, (qp / 8.0))) : 1.0);
#endif
        }
        else
        {
            {
                if (img->typeb == 2)
                {
					lambda_mode   = 0.85 * pow (2, qp/4.0) * max(1.4,min(2, (1+(20.0/qp)))) * (img->type==B_IMG? 4.0:1.0);
                }
                else if ((img->number-intra_frame_number)%HPGOPSIZE==HPGOPSIZE/2)
                {
                    lambda_mode = 0.85 * pow(2, qp / 4.0) * 2 * (img->type == B_IMG ? 4.0 : 1.0) * 0.8;
                }
                else
                {
                    lambda_mode = 0.85 * pow(2, qp / 4.0) * (img->type == B_IMG ? 4.0 : 1.0);
                }
            }
        }

#ifdef TDRDO
		CurMBQP = img->qp;
		if(input->TRDOLength != 0)
		{
			if(GlobeFrameNumber < input->no_frames && img->type!=0)
			{
				if(input->no_frames > StepLength&&GlobeFrameNumber%StepLength==0)
					pOMCPFD = &OMCPDList->FrameDistortionArray[(GlobeFrameNumber-1)/StepLength];
				else
					pOMCPFD = NULL;
			}
			// Just for LDP
			if(img->type != I_IMG)
			{
				if((input->successive_Bframe && GlobeFrameNumber>StepLength) || (!input->successive_Bframe && GlobeFrameNumber>0))
				{
					AdjustLcuQPLambdaLDP(pOMCPFD, img->current_mb_nr, input->img_width/WORKBLOCKSIZE, &CurMBQP, &lambda_mode);
					CurMBQP = max(MIN_QP,min(CurMBQP,MAX_QP));
				}
				if (GlobeFrameNumber%StepLength == 0)
				{
					StoreLCUInf(pRealFD, img->current_mb_nr, input->img_width / WORKBLOCKSIZE, CurMBQP, lambda_mode, img->type); // stores for key frame
				}
			}
		}
#endif

        lambda_motion = sqrt(lambda_mode);
    }

    lambda_motion_factor = LAMBDA_FACTOR(lambda_motion);

    // reset chroma intra predictor to default
    currMB->c_ipred_mode = DC_PRED_8;

    if (bframe)
    {
        Get_IP_direct();
    }

    // Find a motion vector for the Skip mode
	if (img->type == P_IMG)
	{
		FindSkipModeMotionVector();
	}
     
    if (input->rdopt)
    {
        min_rdcost = max_rdcost;
        //===== GET BEST MACROBLOCK MODE =====
        for (ctr16x16 = 0, index = 0; index < 7; index++)
        {
            mode = mb_mode_table[index];
            if (valid[mode])
            {
#ifdef B_DIRECT_REMOVE
                img->NoResidueDirect = 0;
				if (bframe && mode == 0)
				{
					img->NoResidueDirect = 1;
				}
#else
                img->NoResidueDirect = 0;
#endif
                if ((mode == 1 && ctr16x16 == 0) || mode == 2 || mode == 3 || mode == 8)
                {
                    ME(mode, bframe, max_ref, adjust_ref, lambda_motion, write_ref, lambda_motion_factor, valid, lambda_mode, currMB);
                }

                if (img->type == P_IMG && !mode && !img->mv_range_flag)
				{
                    img->mv_range_flag = 1;
                    continue;
                }

                //--- for INTER16x16 check all prediction directions ---
                if (mode == 1 && img->type == B_IMG) 
				{
                    best8x8pdir[1][0] = best8x8pdir[1][1] = best8x8pdir[1][2] = best8x8pdir[1][3] = ctr16x16;
                    
					if (ctr16x16 < 2)
					{
						index--;
					}

                    ctr16x16++;
                }

                if (RDCost_for_macroblocks(lambda_mode, mode, &min_rdcost)) 
				{
                    store_macroblock_parameters(mode);
                }
            }
        }

        set_stored_macroblock_parameters();
    }

    if (img->current_mb_nr == 0)
        intras=0;
    if (img->type==P_IMG && IS_INTRA(currMB))
        intras++;

	if (input->usefme)
	{
		skip_intrabk_SAD(best_mode, max_ref - adjust_ref);
	}
     
}

